
%% 导入数据
opts = delimitedTextImportOptions("NumVariables", 3);

% 指定范围和分隔符
opts.DataLines = [1, Inf];
opts.Delimiter = " ";

% 指定列名称和类型
opts.VariableNames = ["e00", "e1", "e2"];
opts.VariableTypes = ["double", "double", "double"];

% 指定文件级属性
opts.ExtraColumnsRule = "ignore";
opts.EmptyLineRule = "read";
opts.ConsecutiveDelimitersRule = "join";
opts.LeadingDelimitersRule = "ignore";

% 导入数据
trajectory = readtable("C:\Users\ljn\Documents\papers\Dual UWB localization and analysis\simulation\filter\optimization_xyzgamma\64\localization_result_4_anchors_gt.txt", opts);
trajectory = table2array(trajectory);
clear opts


%% plot
% 定义大框和小框的尺寸和中心位置
% 大框的尺寸和中心位置
big_box_size = [3.5, 4.8, 1.6];  % 单位：米
big_box_center = [0, 0, 0];  % 中心位置

% 小框的尺寸和中心位置
small_box_size = [0.5, 0.5, 0.5];  % 单位：米
small_box_center = [0, 0, 0];  % 中心位置

% 大框的顶点坐标
big_box_vertices = [
    -big_box_size(1)/2, -big_box_size(2)/2, 0;
    big_box_size(1)/2, -big_box_size(2)/2, 0;
    big_box_size(1)/2, big_box_size(2)/2, 0;
    -big_box_size(1)/2, big_box_size(2)/2, 0;
    -big_box_size(1)/2, -big_box_size(2)/2, big_box_size(3);
    big_box_size(1)/2, -big_box_size(2)/2, big_box_size(3);
    big_box_size(1)/2, big_box_size(2)/2, big_box_size(3);
    -big_box_size(1)/2, big_box_size(2)/2, big_box_size(3);
];

% 小框的顶点坐标
small_box_vertices = [
    -small_box_size(1)/2, -small_box_size(2)/2, -small_box_size(2)/2;
    small_box_size(1)/2, -small_box_size(2)/2, -small_box_size(2)/2;
    small_box_size(1)/2, small_box_size(2)/2, -small_box_size(2)/2;
    -small_box_size(1)/2, small_box_size(2)/2, -small_box_size(2)/2;
    -small_box_size(1)/2, -small_box_size(2)/2, small_box_size(3)/2;
    small_box_size(1)/2, -small_box_size(2)/2, small_box_size(3)/2;
    small_box_size(1)/2, small_box_size(2)/2, small_box_size(3)/2;
    -small_box_size(1)/2, small_box_size(2)/2, small_box_size(3)/2;
];
edge = [1,2;1,4;1,5;
    3,2;3,4;3,7;
    6,2;6,7;6,5;
    8,4;8,7;8,5]
% 定义轨迹数据和离散点数据

discrete_points = [
%     0, 0, 0;
%     0, 0, 1.65;
    0, 0, 3.3;
    2.0, 0, 3.3;
    4.0, 0, 3.3;
%     4.0, 0, 1.65;
%     4.0, 0, 0.0;
];

% 绘制大框和小框
figure('Units', 'centimeters', 'Position', [0, 0, 8.0, 6.0]);
B_s = big_box_vertices(edge(:,1),:)
B_e = big_box_vertices(edge(:,2),:)
for i = 1:12
    plot3([B_s(i,1), B_e(i,1)], [B_s(i,2), B_e(i,2)], [B_s(i,3), B_e(i,3)], '--','color',[96 96 96]/255*0.5, 'LineWidth', 0.5);
    hold on;
end
% % 绘制小框
% b_s = small_box_vertices(edge(:,1),:)
% b_e = small_box_vertices(edge(:,2),:)
% for i = 1:12
%     plot3([b_s(i,1), b_e(i,1)], [b_s(i,2), b_e(i,2)], [b_s(i,3), b_e(i,3)], '-','color',[96 96 96]/255, 'LineWidth', 0.5);
%     hold on;
% end


% xlim([-3 7]);
% ylim([-3 3]);
daspect([1 1 1]); % 数据纵横比，确保x和y轴比例相同
pbaspect([1 1 0.5]); % 图框纵横比，z轴比例为另外一个值
% title('Boxes and Trajectory');
grid on;
% axis equal;

% 绘制轨迹
sampling_interval = 100
sampled_trajectory = trajectory(1:sampling_interval:end, :);
p1 = plot3(sampled_trajectory(:,1), sampled_trajectory(:,2), sampled_trajectory(:,3), 'b-', 'LineWidth', 1, 'Marker','.');

% 绘制离散点
p2 = scatter3(discrete_points(:,1), discrete_points(:,2), discrete_points(:,3), 16, 'r', 'filled');

% legend('Big Box', 'Small Box', 'Trajectory', 'Discrete Points');

lgd = legend([p1, p2], {'Trajectory', 'Evaluation points'}); % 创建图例
lgd.FontSize = 6; % 设置图例字体大小
lgd.FontName = 'Times'; % 设置字体样式
lgd.Location = 'northeast'; % 将图例放置在右上角
lgd.Box = 'off'; % 去掉图例边框

xlabel('X(m)', 'FontSize', 8, 'FontName', 'Times');
ylabel('Y(m)', 'FontSize', 8, 'FontName', 'Times');
zlabel('Z(m)', 'FontSize', 8, 'FontName', 'Times');

set(gca, 'TickDir', 'in'); %刻度线朝向内侧
set(gca, 'FontSize', 8, 'FontName', 'Times'); % 刻度线的字体大小和字体类型

print(gcf, 'figure_evaluation.png', "-dpng", "-r600");
% hold off;
% close(gcf);